/*
* Copyright (c) 2011 adVolition
*
* Permission is hereby granted, free of charge, to any person
* obtaining a clone of this software and associated documentation
* files (the "Software"), to deal in the Software without
* restriction, including without limitation the rights to use,
* clone, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the
* Software is furnished to do so, subject to the following
* conditions:
*
* The above copyright notice and this permission notice shall be
* included in all copies or substantial portions of the Software.
*
* Except as contained in this notice, the name(s) of the above
* copyright holders shall not be used in advertising or otherwise to
* promote the sale, use or other dealings in this Software without prior
* written authorization.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
* OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
* HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
* WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
* OTHER DEALINGS IN THE SOFTWARE.
 */

// NOTE:  Can't get sonar distance to convert correctly for concave and convex barriers. 3/15/11


package ArenaSimulator;
import NeuralNetwork.*;
import java.awt.Graphics2D;
import java.awt.Color;
import userinterface.Trace;
import com.thoughtworks.xstream.annotations.*;


/**
 * Defines the physical properties of a robot operating in an arena.
 */
@XStreamAlias("robot") public class Robot {
       
    @XStreamAlias("score") @XStreamAsAttribute private Double _score;
    @XStreamAlias("brain") private Brain _brain;
    @XStreamAlias("mutability") private Mutability _fidelity;
    
    private transient Clan _clan;
    
    private transient Vector2D _currentPosition;
    private transient Vector2D _futurePosition;

    private transient Double _currentFacing; // Robot facing in degrees.
    private transient Double _futureFacing;

    private transient Vector2D _robotsXAxis;
    private transient Vector2D _robotsYAxis;

    
    private transient Vector2D _linearVelocityVector;
    private transient Vector2D _linearDisplacementVector;

    
    private transient Double _minimumSonarDistance;

    private transient Double _timeIncrement;
    private transient Double _arenaTime;

    private transient Double _stallTime;

    private transient Trace _sonarTrace;
    private transient Trace _timeTrace;
   
    private transient Boolean _robotIsStuck;
    private transient Boolean _robotHasFallen;

    public Robot(Clan clan) {
        _clan = clan;
        configureObject();
    }


    public Object readResolve() {
        configureObject();
        return this;
    }
    
    /** Used by class constructor and readResolve routines to ensure that newly created
     * objects are configured properly.
     **/
    private void configureObject() {
        
        if (_score == null) _score = -10000.0;
        
        if (_brain == null) _brain = new Brain();
        
        if (_fidelity == null) _fidelity = new Mutability();

        _brain.setFidelity(_fidelity);

        _stallTime = 0.0;
        
        _linearVelocityVector = new Vector2D();
        _linearDisplacementVector = new Vector2D();
        
        _minimumSonarDistance = 0.0;
        
        _currentPosition = new Vector2D();
        _futurePosition = new Vector2D();
        _currentPosition.cartesianSet(80.0,30.0);
        _currentFacing = 0.0;
        _futureFacing = 0.0;
        _robotsXAxis = new Vector2D();
        _robotsYAxis = new Vector2D();

        setRobotIsStuck(false);
        setRobotHasFallen(false);
        
        _timeIncrement = 1.0;
        _arenaTime = 0.0;


        _sonarTrace = new Trace();
        _timeTrace = new Trace();
        _sonarTrace.define("Sonar Distance", 1, "", "cm");
        _timeTrace.define("Time", 1, "", "s");

    }

    public void addScore(Double score) {
        _score += score - (double)size();       
    }

    public Robot getClone() {
        Robot robot = new Robot(_clan);
        robot.setBrain((Brain)_brain.getClone());
        robot.resetScore();
        return robot;
    }

    public void setBrain(Brain newBrain) {
        _brain = newBrain;
    }

    /**
     * Checks to see if the robot hit the ends of the barrier.  If it has hit a barrier.
     * it tells the robot's brain that the motors are not moving.  This function also
     * checks to see if the barrier appears on the robot's sonar and records it's distance if it
     * is the closest barrier seen by the robot so far.
     * @param feature
     * @return
     */
    public Boolean encounteredFeature(Feature feature) {
        // See if barrier shows up on the robot's sonar.
        Double sonarDistance = getSonarDistance(feature);
        if (sonarDistance < _minimumSonarDistance) _minimumSonarDistance = sonarDistance;

        // See if robot has colided with the barrier.
        for (int i = 1; i < feature.getNodeCount(); i ++) {
            if (_futurePosition.getDistanceToLine(feature.getNode(i - 1), feature.getNode(i)) <= _clan.getRobotRadius()) {
                _futurePosition.vectorSet(_currentPosition);
                return true;
            }
        }
        return false;
    }

    /**
     * Sets the current facing of the robot.
     * @param newValue (degrees)
     */
    public void setCurrentFacing(Double newValue) {
        
        while (newValue < 0.0) {
            newValue += 360.0;
        }
        while (newValue > 360.0) {
            newValue -= 360.0;
        }
        
        _currentFacing = newValue;
        _futureFacing = newValue;
        _linearVelocityVector.polarSet(_clan.getRobotLinearVelocity(), _currentFacing);
        _robotsYAxis.polarSet(1.0, _currentFacing);
        _robotsXAxis.polarSet(1.0, _currentFacing + 90.0);
    }
    
    /**
     * Sets the current position of the robot.  Increments the stall time if the
     * robot has not changed its position.
     * @param newPosition (centimeters)
     */
    private void setCurrentPosition(Vector2D newPosition) {
        if (_currentPosition.equals(newPosition)) {
            _stallTime += _timeIncrement;
        } else {
            _currentPosition.vectorSet(newPosition);
            _stallTime = 0.0;
        }
    }

    public void setRobotIsStuck(Boolean status) {
        _robotIsStuck = status;
    }
    
    public void setRobotHasFallen(Boolean status) {
        _robotHasFallen = status;
    }
    
    /**
     * Returns the total number of nodes and edges contained in the robot's brain.
     * @return 
     */
    public int size() {
        return _brain.size();
    }
    
    /**
     * Computes the future state of the robot.  
     * @param timeIncrement
     */
    public void computeFutureState(Double timeIncrement) {
        
        _timeIncrement = timeIncrement;
        _brain.setSonarNode(computeSonarNodeValue());
        _brain.computeFutureState(timeIncrement);
        if (_brain.moveForward()) {computeForwardMotion(timeIncrement);}
        if (_brain.moveBackward()) {computeBackwardMotion(timeIncrement);}
        if (_brain.turnRight()) {computeRightRotation(timeIncrement);}
        if (_brain.turnLeft()) {computeLeftRotation(timeIncrement);}

        //computeForwardMotion(timeIncrement);
        _minimumSonarDistance = _clan.getRobotRestingSonarDistance();
    }

    private void computeForwardMotion(Double timeIncrement) {
        _linearDisplacementVector.vectorSet(_linearVelocityVector);
        _linearDisplacementVector.multiplyBy(timeIncrement);
        _futurePosition.addVector(_linearDisplacementVector);
    }

    private void computeBackwardMotion(Double timeIncrement) {
        _linearDisplacementVector.vectorSet(_linearVelocityVector);
        _linearDisplacementVector.multiplyBy(timeIncrement);
        _futurePosition.subtractVector(_linearDisplacementVector);
    }

    private void computeRightRotation(Double timeIncrement) {
        _futureFacing += timeIncrement * _clan.getRobotAngularVelocity();
    }

    private void computeLeftRotation(Double timeIncrement) {
        _futureFacing -= timeIncrement * _clan.getRobotAngularVelocity();
    }

    private Double computeSonarNodeValue() {
        if (_minimumSonarDistance <= _clan.getRobotRestingSonarDistance()) {
            return _minimumSonarDistance / _clan.getRobotRestingSonarDistance() * 0.6;
        }
        return 0.8;
    }

    /**
     * Returns the average performance score for the robot.
     * @return
     */
    public Double getScore() {
        return _score;
    }

    /**
     * Returns the distance in centimeters from the center of the robot to
     * barrier in front of the robot.
     * @param feature
     * @return
     */
    private Double getSonarDistance(Feature feature) {
        Vector2D pointA;
        Vector2D pointB;

        for (int i = 1; i < feature.getNodeCount(); i ++) {
            pointA = feature.getNode(i - 1).copy();
            pointB = feature.getNode(i).copy();

            pointA.transformCoordinates(_currentPosition, _robotsXAxis, _robotsYAxis);
            pointB.transformCoordinates(_currentPosition, _robotsXAxis, _robotsYAxis);

            if (pointA.getX() * pointB.getX() <= 0.0) { // Confirm the target crosses the y axis.
                Double distance = getYIntercept(pointA, pointB) - _clan.getRobotRadius();
                if (distance >= 0.0) {
                    //if (distance < 20.0) _linearVelocityVector.cartesianSet(0.0, 0.0);
                    if (feature.isWall()) {
                        return distance; 
                    } else if (feature.isLedge()) {
                        return 10000000.0; // The sonar is looking over an edge.  Distance is essentially infinate.
                    } //
                }
            }
        }
        return _clan.getRobotRestingSonarDistance();
    }

    /**
     * Returns the amount of time that the robot has been stalled.
     * @return
     */
    public Double getStallTime() {
        return _stallTime;
    }

    /**
     * Returns the y intercept of a line defined by pointA and pointB.
     * @param pointA
     * @param pointB
     * @return
     */
    private Double getYIntercept(Vector2D pointA, Vector2D pointB) {

        //y = y1 + [(y2 - y1) / (x2 - x1)]·(x - x1)  where x = 0

        Double x1 = pointA.getX();
        Double y1 = pointA.getY();
        Double x2 = pointB.getX();
        Double y2 = pointB.getY();

        return y1 - x1 * (y2 - y1)/(x2 - x1);
    }


    public void implementFutureState() {
        // Robot only moves forward if it hasn't hit an obsticle.
        if (! _robotIsStuck && ! _robotHasFallen) {
            setCurrentPosition(_futurePosition);
            setCurrentFacing(_futureFacing);
        }
        _brain.implementFutureState();
        _arenaTime += _timeIncrement;
        updateTraces();
    }

    /**
     * Returns the radian value of angle expressed in degrees.
     * @param degrees
     * @return
     */
    private double getRadians(Double degrees) {
        return degrees / 180.0 * Math.PI;
    }

    /**
     * Redraws the robot.
     * @param xOffset
     * @param yOffset
     * @param scale
     * @param g2d
     */
    public void draw(int xOffset, int yOffset, Double scale, Graphics2D g2d) {
        int diameter = (int) (_clan.getRobotRadius() * 2.0 / scale);
        int aX = _currentPosition.getScaledX(scale) + xOffset;
        int aY = _currentPosition.getScaledY(scale) + yOffset;

        g2d.setColor(Color.black);
        g2d.drawArc(aX - diameter / 2, aY - diameter / 2, diameter, diameter, 0, 360);
    
        // Draw the facing indicator.
        int bX = aX + (int)(_clan.getRobotRadius() * Math.sin(getRadians(_currentFacing)) / scale);
        int bY = aY + (int)(_clan.getRobotRadius() * Math.cos(getRadians(_currentFacing)) / scale);
        g2d.drawLine(aX, aY, bX, bY);

        // Draw traces.
        // _sonarTrace.redraw(g2d);
        // _timeTrace.redraw(g2d);

    }
    
    /**
     * Resets the robot after every run.
     */
    public void reset() {
        _robotIsStuck = false;
        _robotHasFallen = false;
        _brain.reset();
        _stallTime = 0.0;
        
    }
    
    
    public void resetScore() {
        // Penalize robot for size of brain.
        _score = 0.0;
    }
    
    /**
     * Sets the clan to which the robot belongs.
     * @param clan 
     */
    public void setClan (Clan clan) {
        _clan = clan;
    } 

    /**
     * Sets the robot's x and y position in centimeters.
     * @param x
     * @param y
     */
    public void setPosition(Double x, Double y) {
        _currentPosition.cartesianSet(x, y);
        _futurePosition.cartesianSet(x, y);
    }

    /**
     * This function is called to update traces on robot variables.
     */
    public void updateTraces() {
       _sonarTrace.update(_minimumSonarDistance);
       _timeTrace.update(_arenaTime);
    }
}

